#include <math.h>
#include "coding.h"
#include "../util.h"

/**
 * Sony's HEVAG (High Efficiency VAG) ADPCM, used in Vita/PS4 games (hardware decoded).
 * Evolution of the regular VAG (same flags and frames), uses 4 history samples and a bigger table.
 *
 * Reverse engineered from PC tools, base int code by daemon1 (using K=2^13 aka coefs * 8192, 0.9375 > 7680,
 * ).
 *
 * HEVAG is made to be compatible with original VAG, so table indexes <= 0x1c behave like old codec
 * setting hist3/4 coefs to 0 (as a minor optimization og code won't mult hist3/4 in lower table indexes).
 * 
 * Original code uses SIMD to do 4 codes at once (unrolled doing 7 groups of 4 codes = 28 samples).
 * Tables are rather complex to (presumable) handle hist dependencies, and since it's VAG compatible
 * should be the same (maybe rounding diffs?). Hist and output are floats (output converted to +-1.0
 * or +-32768.0 +-0.5 based on a flag). Code below just does it linearly 1 by 1 in pcm16.
 */

/* ADPCM table */
static const float hevag_coefs_f[128][4] = {
    {-0.0f, -0.0f, -0.0f, -0.0f },
    { 0.9375f, -0.0f, -0.0f, -0.0f },
    { 1.796875f, -0.8125f, -0.0f, -0.0f },
    { 1.53125f, -0.859375f, -0.0f, -0.0f },
    { 1.90625f, -0.9375f, -0.0f, -0.0f },
    { 1.7982178f, -0.86169434f, -0.0f, -0.0f },
    { 1.770874f, -0.89916992f, -0.0f, -0.0f },
    { 1.6992188f, -0.91821289f, -0.0f, -0.0f },
    { 1.6031494f, -0.9375f, -0.0f, -0.0f },
    { 1.4682617f, -0.9375f, -0.0f, -0.0f },
    { 1.3139648f, -0.9375f, -0.0f, -0.0f },
    { 1.1424561f, -0.9375f, -0.0f, -0.0f },
    { 0.95605469f, -0.9375f, -0.0f, -0.0f },
    { 0.75695801f, -0.9375f, -0.0f, -0.0f },
    { 0.54785156f, -0.9375f, -0.0f, -0.0f },
    { 0.33166504f, -0.9375f, -0.0f, -0.0f },
    { 0.11108398f, -0.9375f, -0.0f, -0.0f },
    {-0.11108398f, -0.9375f, -0.0f, -0.0f },
    {-0.33166504f, -0.9375f, -0.0f, -0.0f },
    {-0.54785156f, -0.9375f, -0.0f, -0.0f },
    {-0.75695801f, -0.9375f, -0.0f, -0.0f },
    {-0.95605469f, -0.9375f, -0.0f, -0.0f },
    {-1.1424561f, -0.9375f, -0.0f, -0.0f },
    {-1.3139648f, -0.9375f, -0.0f, -0.0f },
    {-1.4682617f, -0.9375f, -0.0f, -0.0f },
    {-1.6031494f, -0.9375f, -0.0f, -0.0f },
    {-1.6992188f, -0.91821289f, -0.0f, -0.0f },
    {-1.770874f, -0.89916992f, -0.0f, -0.0f },
    {-1.7982178f, -0.86169434f, -0.0f, -0.0f },
    { 0.65625f, -1.125f, 0.40625f, -0.375f },
    {-0.78125f, -0.875f, -0.40625f, -0.28125f },
    {-1.28125f, -0.90625f, -0.4375f, -0.125f },
    {-0.020385742f, -0.33227539f, -0.060302734f, -0.066040039f },
    {-0.90698242f, -0.27111816f, -0.28051758f, 0.051757812f },
    {-0.97668457f, -0.38647461f, -0.34350586f, 0.03527832f },
    { 0.73461914f, -0.57983398f, 0.32336426f, -0.15844727f },
    { 0.46362305f, -0.84790039f, 0.47302246f, -0.1484375f },
    {-1.0054932f, -0.31689453f, -0.25280762f, 0.027709961f },
    { 1.1229248f, 0.24194336f, -0.16870117f, -0.28271484f },
    { 1.5894775f, -0.37158203f, -0.46289062f, 0.15466309f },
    { 1.6005859f, -0.54772949f, -0.2746582f, 0.20324707f },
    {-0.20361328f, -0.45703125f, -0.78808594f, 0.10253906f },
    { 0.95446777f, -0.52832031f, 0.25769043f, -0.061767578f },
    { 1.168335f, -0.16308594f, -0.092407227f, 0.059448242f },
    { 1.2246094f, -0.31274414f, 0.036621094f, 0.024291992f },
    {-0.57922363f, -0.50317383f, -0.66967773f, -0.18225098f },
    {-0.71972656f, 0.2902832f, -0.58435059f, -0.84802246f },
    {-0.14562988f, -1.112915f, -0.15100098f, -0.38012695f },
    { 0.33972168f, -0.86767578f, -0.19226074f, -0.17663574f },
    {-0.89526367f, -0.25170898f, -0.27001953f, 0.054443359f },
    { 0.7479248f, -0.3145752f, -0.038452148f, -0.0021972656f },
    { 1.1544189f, -0.22680664f, 0.012451172f, 0.031494141f },
    { 0.96142578f, -0.54724121f, 0.25952148f, -0.065673828f },
    {-0.87548828f, -0.21911621f, -0.25256348f, 0.058837891f },
    {-0.89819336f, -0.2565918f, -0.27258301f, 0.053710938f },
    {-1.1193848f, -0.42834473f, -0.32641602f, -0.047729492f },
    {-0.32202148f, -0.32312012f, -0.23547363f, -0.1998291f },
    { 0.2286377f, 1.1209717f, 0.22705078f, -0.70141602f },
    { 1.1247559f, 0.22692871f, -0.13720703f, -0.29626465f },
    { 1.6118164f, -0.36767578f, -0.50524902f, 0.16723633f },
    { 1.5181885f, -0.58496094f, -0.03125f, 0.075927734f },
    {-0.32385254f, -0.13964844f, -0.38842773f, -0.83959961f },
    { 1.1390381f, -0.12792969f, -0.10107422f, 0.061889648f },
    { 0.20043945f, -0.075683594f, -0.11547852f, -0.51623535f },
    { 0.51831055f, -0.92590332f, -0.065063477f, -0.27575684f },
    {-1.097168f, -0.47497559f, -0.34265137f, 0.0053710938f },
    {-0.31274414f, -0.3338623f, -0.21118164f, -0.23181152f },
    { 0.38842773f, -0.058959961f, -0.087158203f, -0.17346191f },
    { 0.96887207f, -0.46923828f, 0.34436035f, -0.12438965f },
    { 1.229126f, -0.31848145f, 0.038330078f, 0.023803711f },
    { 1.0253906f, -0.40246582f, 0.18933105f, -0.018920898f },
    {-1.0411377f, -0.33874512f, -0.296875f, -0.041015625f },
    { 1.1568604f, -0.22973633f, 0.013183594f, 0.03125f },
    { 0.0091552734f, -0.27355957f, -0.036376953f, -0.84680176f },
    {-1.1160889f, -0.5078125f, -0.36169434f, 0.00061035156f },
    {-0.88745117f, -0.23901367f, -0.26318359f, 0.056152344f },
    {-0.33447266f, 0.45715332f, 0.72460938f, -0.13293457f },
    { 1.0977783f, 0.23779297f, -0.083374023f, -0.33007812f },
    { 1.5992432f, -0.34606934f, -0.47045898f, 0.12878418f },
    { 1.164917f, -0.23937988f, 0.015869141f, 0.030517578f },
    { 0.64355469f, -0.52124023f, 0.38134766f, -0.38537598f },
    {-0.93945312f, -0.41296387f, -0.3548584f, -0.055664062f },
    { 0.89221191f, 0.3079834f, 0.052978516f, -0.30041504f },
    { 1.2542725f, -0.34997559f, 0.047729492f, 0.020996094f },
    { 1.3354492f, -0.45422363f, 0.081176758f, 0.01184082f },
    { 0.0029296875f, -0.037841797f, -0.15405273f, 0.0390625f },
    {-0.99145508f, -0.29431152f, -0.28210449f, -0.033081055f },
    {-1.0389404f, -0.37438965f, -0.28527832f, 0.019897461f },
    { 0.039794922f, -0.46948242f, 0.051147461f, -0.1138916f },
    { 1.0858154f, 0.26782227f, -0.066040039f, -0.3515625f },
    { 1.4737549f, -0.22900391f, -0.24621582f, -0.073364258f },
    { 1.0655518f, -0.41784668f, 0.2043457f, -0.020629883f },
    { 1.5808105f, -0.46960449f, -0.36706543f, 0.23754883f },
    { 1.2253418f, -0.3137207f, 0.036865234f, 0.024169922f },
    { 1.1456299f, -0.33654785f, 0.12304688f, 0.0050048828f },
    {-0.57617188f, -0.61108398f, -0.34814453f, -0.14172363f },
    { 0.96057129f, -0.52807617f, 0.26062012f, -0.061157227f },
    { 0.29907227f, -1.0494385f, 0.15856934f, -0.33935547f },
    { 1.2441406f, -0.33728027f, 0.043945312f, 0.022094727f },
    { 1.3809814f, -0.51428223f, 0.10168457f, 0.0064697266f },
    { 1.239502f, -0.33154297f, 0.042114258f, 0.022583008f },
    { 1.1765137f, -0.17297363f, -0.08996582f, 0.058837891f },
    { 0.47045898f, -0.5559082f, 0.3470459f, -0.41467285f },
    { 0.81774902f, -0.6907959f, 0.27453613f, -0.13110352f },
    { 1.3527832f, -0.47705078f, 0.088867188f, 0.009765625f },
    {-0.12524414f, -1.1975098f, -0.098266602f, -0.42260742f },
    { 1.269043f, -0.45727539f, 0.16687012f, -0.01171875f },
    { 1.2557373f, 0.12060547f, -0.23376465f, -0.17541504f },
    { 0.9708252f, 0.47338867f, -0.093261719f, -0.39831543f },
    { 1.5489502f, -0.4119873f, -0.40942383f, 0.25378418f },
    { 0.81066895f, 0.38647461f, 0.028198242f, -0.25500488f },
    {-0.28662109f, -0.89770508f, -0.23730469f, -0.50317383f },
    { 1.1340332f, -0.49304199f, 0.23010254f, -0.030029297f },
    { 0.56555176f, -0.78161621f, 0.21337891f, -0.19763184f },
    { 1.3729248f, -0.50354004f, 0.097900391f, 0.0074462891f },
    { 1.1971436f, -0.27880859f, 0.026733398f, 0.027099609f },
    { 1.1884766f, -0.1875f, -0.086181641f, 0.057739258f },
    { 1.0302734f, -0.41943359f, 0.19067383f, -0.021484375f },
    { 1.1361084f, -0.12463379f, -0.10192871f, 0.062133789f },
    { 0.20727539f, -1.1016846f, 0.083984375f, -0.37072754f },
    { 1.2468262f, -0.34069824f, 0.044921875f, 0.021850586f },
    { 1.0241699f, 0.39648438f, -0.092529297f, -0.36486816f },
    { 0.87902832f, 0.40478516f, 0.0056152344f, -0.3190918f },
    {-0.010742188f, -0.95324707f, -0.065673828f, -0.5579834f },
    { 0.75598145f, -0.63342285f, 0.33691406f, -0.15197754f },
    { 1.5045166f, -0.1574707f, -0.40087891f, 0.030883789f },
    { 1.5947266f, -0.49743652f, -0.34472656f, 0.22912598f },
    { 0.65100098f, 0.36608887f, 0.094604492f, -0.13818359f },
};

#if 0
/* original scale used instead of "shift = 20 - i"
 * adjusted for +-1.0 floats, so if used for pcm16 needs to be scaled up ([i] = 1.0 / 128.0*i) */
static const float scale_table_f[16] = {
    0.0078125f, 0.00390625f, 0.001953125f, 0.0009765625f,
    0.00048828125f, 0.000244140625f, 0.0001220703125f, 0.00006103515625f,
    0.000030517578125f, 0.0000152587890625f, 0.00000762939453125f, 0.000003814697265625f,
    0.0000019073486328125f, 0.00000095367431640625f, 0.000000476837158203125f, 0.000000238418579101562f,
};
#endif


void decode_hevag(VGMSTREAMCHANNEL* stream, sample_t* outbuf, int channelspacing, int32_t first_sample, int32_t samples_to_do) {
    uint8_t frame[0x10] = {0};
    off_t frame_offset;
    int i, frames_in, sample_count = 0;
    size_t bytes_per_frame, samples_per_frame;
    int index, shift, flag;
    int32_t hist1 = stream->adpcm_history1_32;
    int32_t hist2 = stream->adpcm_history2_32;
    int32_t hist3 = stream->adpcm_history3_32;
    int32_t hist4 = stream->adpcm_history4_32;


    /* external interleave (fixed size), mono */
    bytes_per_frame = 0x10;
    samples_per_frame = (bytes_per_frame - 0x02) * 2; /* always 28 */
    frames_in = first_sample / samples_per_frame;
    first_sample = first_sample % samples_per_frame;

    /* parse frame header */
    frame_offset = stream->offset + bytes_per_frame * frames_in;
    read_streamfile(frame, frame_offset, bytes_per_frame, stream->streamfile); /* ignore EOF errors */
    index = (frame[0] >> 4) & 0xf;
    shift = (frame[0] >> 0) & 0xf;
    index = ((frame[1] >> 0) & 0xf0) | index;
    flag = (frame[1] >> 0) & 0xf; /* same flags */

    VGM_ASSERT_ONCE(index > 127 || shift > 12, "HEVAG: in+correct coefs/shift at %x\n", (uint32_t)frame_offset);
    if (index > 127) /* not validated so would read garbage, simulate with 0 */
        index = 0;
    //if (shift > 12) /* shouldn't happen but accepted in scale_table (just zeroes codes) */
    //    shift = 9;


    /* decode nibbles */
    for (i = first_sample; i < first_sample + samples_to_do; i++) {
        int32_t code, sample = 0;

        if (flag < 0x07) { /* with flag 0x07 decoded sample must be 0 */
            uint8_t nibbles = frame[0x02 + i/2];

            code = (i&1 ? /* low nibble first */
                    get_high_nibble_signed(nibbles):
                    get_low_nibble_signed(nibbles));

            //code = (code << 0x4) * scale_table_f[shift]; /* OG s8 sign extension w/ scale table (+-1.0) */
            code = ((code << 12) >> shift);
            sample = hist1 * hevag_coefs_f[index][0] +
                     hist2 * hevag_coefs_f[index][1] +
                     hist3 * hevag_coefs_f[index][2] +
                     hist4 * hevag_coefs_f[index][3];
            sample = code + sample; /* no clamping here */
#if 0
            // base int code
            sample = code << (20 - shift_factor);
            sample = (hists... * coefs....) >> 5) + sample;
            sample >>= 8;
#endif
        }

        outbuf[sample_count] = clamp16(sample);
        sample_count += channelspacing;

        hist4 = hist3;
        hist3 = hist2;
        hist2 = hist1;
        hist1 = sample;
    }

    stream->adpcm_history1_32 = hist1;
    stream->adpcm_history2_32 = hist2;
    stream->adpcm_history3_32 = hist3;
    stream->adpcm_history4_32 = hist4;
}
